/**************************************************************
*
* M3D_TRS.CPP
*
* Transformations based on quaternions support
**************************************************************/

#include <m3d/m3d_stdafx.h>

#include <ap_comm.h>
#include <m3d.h>

m3dTRS m3dTrsIdentity(m3dVZero, m3dQUAT(), m3dVOneXYZ);

/**************************************************************
* m3dTRS::Identity()
*
**************************************************************/
void m3dTRS::Identity (void)
{
   t = m3dVZero;
   r.Identity();
   s = m3dVOneXYZ;
}

/**************************************************************
* m3dTRS::Invert()
*
**************************************************************/
void m3dTRS::Invert (void)
{
   s = m3dVOneXYZ / s;
   r.Invert();
   _m3dMultiplyVector(&t, &s);
   r.Transform(&t);
   m3dNegateVector(&t);
}

/**************************************************************
* m3dTRS::Inverse()
*
**************************************************************/
void m3dTRS::Invert (m3dTRS *res) const
{
   *res = *this;
   res->Invert();
}

/**************************************************************
* m3dTRS::SetTransform()
*
**************************************************************/
void m3dTRS::SetTransform (const m3dV* transVec, const m3dQUAT* rotateQuat, const m3dV* scaleVec)
{
   SetScale(scaleVec);
   SetRotation(rotateQuat);
   SetTranslation(transVec);
}

/**************************************************************
* m3dTRS::SetTranslation()
*
**************************************************************/
void m3dTRS::SetTranslation (const m3dV* transVec)
{
   t.x = transVec->x;
   t.y = transVec->y;
   t.z = transVec->z;
}

/**************************************************************
* m3dTRS::SetRotation()
*
**************************************************************/
void m3dTRS::SetRotation (const m3dQUAT* rotateQuat)
{
   r.s = rotateQuat->s;
   r.v = rotateQuat->v;
}

/**************************************************************
* m3dTRS::SetScale()
*
**************************************************************/
void m3dTRS::SetScale (const m3dV* scaleVec)
{
   s.x = scaleVec->x;
   s.y = scaleVec->y;
   s.z = scaleVec->z;
}

/**************************************************************
* m3dTRS::SetMatr()
*
**************************************************************/
void m3dTRS::SetMatr (const m3dMATR* matr)
{
   r.SetMatr(matr);
   matr->GetScale(&s.x, &s.y, &s.z);
   matr->GetTranslate(&t);
}

/**************************************************************
* m3dTRS::GetMatr()
*
**************************************************************/
void m3dTRS::GetMatr (m3dMATR *matr) const
{
   r.GetMatr(matr);
   matr->Scale(s.x, s.y, s.z, M3D_PRE);
   matr->Translate(&t, M3D_POST);
}

/**************************************************************
* m3dTRS::Transform()
*
**************************************************************/
void m3dTRS::Transform (m3dV *vct) const
{
   vct->x *= s.x;
   vct->y *= s.y;
   vct->z *= s.z;
   r.Transform(vct);
   vct->x += t.x;
   vct->y += t.y;
   vct->z += t.z;
}

/**************************************************************
* m3dTRS::Transform()
*
**************************************************************/
void m3dTRS::Transform (const m3dV *a, m3dV *res) const
{
   res->x = a->x * s.x;
   res->y = a->y * s.y;
   res->z = a->z * s.z;
   r.Transform(res);
   res->x += t.x;
   res->y += t.y;
   res->z += t.z;
}

/**************************************************************
* m3dIsEqualTrs_A()
*
**************************************************************/
BOOL m3dIsEqualTrs_A (const m3dTRS *a, const m3dTRS *b, float acc)
{
   if (!m3dIsEqualPoint_A(&a->t, &b->t, acc)) {
      return FALSE;
   }
   if (!m3dIsEqualPoint_A(&a->s, &b->s, acc)) {
      return FALSE;
   }
   if (!m3dIsEqualQuat_A(&a->r, &b->r, acc)) {
      return FALSE;
   }
   return TRUE;
}

//
// End of file 'M3D_TRS.CPP'
//
